#include <algorithm>
#include <ctrl_alg/pid/pid.hpp>
#include <ctrl_alg/pid/pid_pos.hpp>
#include <eigen3/Eigen/Geometry>
#include <general_interface/action/auto_task.hpp>
#include <general_interface/msg/chassis_servo.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

class ConnectionTaskNode : public rclcpp::Node
{
  private:
    using Task = general_interface::action::AutoTask;
    using ChassisServo = general_interface::msg::ChassisServo;
    rclcpp_action::Server<Task>::SharedPtr server;
    std::unique_ptr<tf2_ros::Buffer> buffer;
    std::shared_ptr<tf2_ros::TransformListener> tf;
    rclcpp::TimerBase::SharedPtr timer;
    rclcpp::Publisher<ChassisServo>::SharedPtr localServo;
    rclcpp::Publisher<ChassisServo>::SharedPtr coopServo;
    std::unique_ptr<CtrlAlg::PID<double>> gyroPID;
    std::unique_ptr<CtrlAlg::PID<double>> distancePID;

    double angleErrorTolerance = 1;

  public:
    ConnectionTaskNode() : Node("connection_task_node")
    {
        server = rclcpp_action::create_server<Task>(
            this, "connect_task",
            std::bind(&ConnectionTaskNode::OnGoal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&ConnectionTaskNode::OnCancel, this, std::placeholders::_1),
            std::bind(&ConnectionTaskNode::OnAccepted, this, std::placeholders::_1));
        localServo = create_publisher<ChassisServo>("chassis_servo", 1);
        // TODO topic name
        coopServo = create_publisher<ChassisServo>("/robot1/chassis_servo", 1);
        buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
        tf = std::make_shared<tf2_ros::TransformListener>(*buffer);
        timer = create_wall_timer(std::chrono::milliseconds(100), std::bind(&ConnectionTaskNode::OnTick, this));

        declare_parameter<double>("tolerance", 1);
        angleErrorTolerance = get_parameter("tolerance").as_double();
        declare_parameter<std::vector<double>>("gyro_PID", {1, 0, 0});
        declare_parameter<std::vector<double>>("distance_PID", {1, 0, 0});
        auto gyroPIDParams = get_parameter("gyro_PID").as_double_array();
        auto distancePIDParams = get_parameter("distance_PID").as_double_array();
        gyroPID = std::make_unique<CtrlAlg::PIDPos<double>>(gyroPIDParams[0], gyroPIDParams[1], gyroPIDParams[2]);
        distancePID =
            std::make_unique<CtrlAlg::PIDPos<double>>(distancePIDParams[0], distancePIDParams[1], distancePIDParams[2]);
    }

  private:
    rclcpp_action::GoalResponse OnGoal(const rclcpp_action::GoalUUID &, std::shared_ptr<const Task::Goal> goal)
    {
        assert(goal->task_id == goal->CONNECT);
        try
        {
            auto c2t = buffer->lookupTransform("target", "camera", tf2::TimePointZero);
            CheckDelay(c2t, 100);
            RCLCPP_DEBUG(get_logger(), "accepted,tf:%f,%f,%f", c2t.transform.translation.x, c2t.transform.translation.y,
                         c2t.transform.translation.z);
            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_WARN(this->get_logger(), "Could not transform : %s", ex.what());
            return rclcpp_action::GoalResponse::REJECT;
        }
    }
    rclcpp_action::CancelResponse OnCancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Task>>)
    {
        return rclcpp_action::CancelResponse::ACCEPT;
    }
    void OnAccepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Task>>) {}

    /**
     * 检查给定变换消息的时间戳是否超过指定范围
     * @param trans TransformStamped消息，包含了需要检查的时间戳
     * @param range 时间范围（以毫秒为单位），用于比较时间戳是否过期
     * @return 如果时间戳超过指定范围或者时间戳为0，则返回true；否则返回false
     */
    bool CheckDelay(const geometry_msgs::msg::TransformStamped &trans, double range)
    {
        auto t = trans.header.stamp;
        return (get_clock()->now().nanoseconds() - t.nanosec) / 1000.0 > range;
    }
    void OnTick()
    {
        try
        {
            auto c2t = buffer->lookupTransform("tag", "camera", tf2::TimePointZero);
            // TODO: implement a better delay check, curent is error
            //if (CheckDelay(c2t, 1000))
            //{
            //    RCLCPP_WARN(get_logger(), "Delay too long");
            //    SetChassisSpeed(0, 0, 0, 0);
            //    return;
            //}

            double x = -c2t.transform.translation.x;
            double y = -c2t.transform.translation.z;
            double angle = std::atan2(-x, y);
            double distance = std::sqrt(x * x + y * y);
            // RCLCPP_INFO(this->get_logger(), "angle:%f", angle * 180 / M_PI);
            // RCLCPP_INFO(get_logger(), "tf:%f,%f,%f", c2t.transform.translation.x, c2t.transform.translation.y,
            //             c2t.transform.translation.z);
            // TODO: we should first project z aix on ground plane, then check out the angle.
            Eigen::Quaterniond q(c2t.transform.rotation.w, c2t.transform.rotation.x, c2t.transform.rotation.y,
                                 c2t.transform.rotation.z);
            Eigen::Vector3d rpy = q.matrix().eulerAngles(1, 0, 2);
            double angleTag = rpy.x();
            (void)angleTag;
            // rpy *= 180 / M_PI;
            // RCLCPP_INFO(get_logger(), "rpy:%f,%f,%f", rpy.x(), rpy.y(), rpy.z());
            double angleError = std::abs(angle);
            double gyroCtrl = -gyroPID->GetControl(angle, 0.1);
            double angleCtrl = distancePID->GetControl(distance, 0.1);
            angleCtrl = std::clamp(angleCtrl, -10.0, 10.0);
            // publish it to the local robot and coopreate robot
            RCLCPP_INFO(get_logger(), "angE,%f", angleError);
            if (angleError > angleErrorTolerance / 180 * M_PI)
                SetChassisSpeed(0, gyroCtrl, 0, gyroCtrl);
            else
                SetChassisAngle(angleCtrl, gyroCtrl, -angleCtrl, gyroCtrl);
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_WARN(this->get_logger(), "Could not transform : %s", ex.what());
            SetChassisSpeed(0, 0, 0, 0);
        }
    }

    void SetChassisAngle(double selfAngle, double selfGyro, double coopAngle, double coopGyro)
    {
        ChassisServo msg;
        msg.speed = 0;
        msg.angle = selfAngle;
        msg.gyro = 0;
        msg.gyroz = selfGyro;
        msg.speed_gain = 0;
        msg.angle_gain = 1;
        msg.gyro_gain = 0;
        msg.gyroz_gain = 1;
        localServo->publish(msg);
        msg.angle = coopAngle;
        msg.gyroz = coopGyro;
        coopServo->publish(msg);
    }

    void SetChassisSpeed(double selfSpeed, double selfGyro, double coopSpeed, double coopGyro)
    {
        ChassisServo msg;
        msg.speed = selfSpeed;
        msg.angle = 0;
        msg.gyro = selfGyro;
        msg.gyroz = 0;
        msg.speed_gain = 1;
        msg.angle_gain = 0;
        msg.gyro_gain = 0;
        msg.gyroz_gain = 1;
        localServo->publish(msg);
        msg.angle = coopSpeed;
        msg.gyroz = coopGyro;
        coopServo->publish(msg);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ConnectionTaskNode>());
    rclcpp::shutdown();
    return 0;
}